#include <iostream>
#include <modbus/modbus.h>
#include <cstring>
#include <csignal>
#include <unistd.h>

modbus_t *ctx = nullptr;
modbus_mapping_t *mb_mapping = nullptr;

void cleanup(int signum)
{
    if (ctx) {
        modbus_close(ctx);
        modbus_free(ctx);
    }
    if (mb_mapping) {
        modbus_mapping_free(mb_mapping);
    }
    exit(signum);
}

int main()
{
    signal(SIGINT, cleanup);
    signal(SIGTERM, cleanup);

    ctx = modbus_new_tcp("0.0.0.0", 502);
    if (ctx == nullptr) {
        std::cerr << "Unable to create the libmodbus context\n";
        return -1;
    }

    mb_mapping = modbus_mapping_new(0, 0, 32, 0); // Create a new mapping with 32 holding registers
    if (mb_mapping == nullptr) {
        std::cerr << "Failed to allocate the mapping: " << modbus_strerror(errno) << "\n";
        modbus_free(ctx);
        return -1;
    }

    int server_socket = modbus_tcp_listen(ctx, 1);
    if (server_socket == -1) {
        std::cerr << "Unable to listen TCP: " << modbus_strerror(errno) << "\n";
        modbus_free(ctx);
        return -1;
    }

    std::cout << "Modbus server is listening...\n";

    while (true) {
        modbus_tcp_accept(ctx, &server_socket);

        uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
        int rc;
        while ((rc = modbus_receive(ctx, query)) > 0) {
            modbus_reply(ctx, query, rc, mb_mapping);

            // Print received data from holding registers
            std::cout << "Received data:\n";
            for (int i = 0; i < 10; i++) {
                std::cout << "Register " << i << ": " << mb_mapping->tab_registers[i] << "\n";
            }

            // Extract received data
            uint64_t timestamp = (static_cast<uint64_t>(mb_mapping->tab_registers[0]) << 48) |
                                (static_cast<uint64_t>(mb_mapping->tab_registers[1]) << 32) |
                                (static_cast<uint64_t>(mb_mapping->tab_registers[2]) << 16) |
                                static_cast<uint64_t>(mb_mapping->tab_registers[3]);

            uint16_t current_measurement = mb_mapping->tab_registers[4];

            double real_speed;
            std::memcpy(&real_speed, &mb_mapping->tab_registers[5], sizeof(real_speed));

            uint16_t dir = mb_mapping->tab_registers[9];

            double kalman_dis;
            std::memcpy(&kalman_dis, &mb_mapping->tab_registers[10], sizeof(kalman_dis));

            double kalman_vel;
            std::memcpy(&kalman_vel, &mb_mapping->tab_registers[14], sizeof(kalman_vel));

            // Print the parsed data
            std::cout << "Received data:\n";
            std::cout << "Timestamp: " << timestamp << "\n";
            std::cout << "Current Measurement: " << current_measurement << "\n";
            std::cout << "Real Speed: " << real_speed << "\n";
            std::cout << "Direction: " << dir << "\n";
            std::cout << "Kalman Dis: " << kalman_dis << "\n";
            std::cout << "Kalman Vel: " << kalman_vel << "\n";
        }

        if (rc == -1) {
            std::cerr << "Connection closed or error: " << modbus_strerror(errno) << "\n";
            modbus_close(ctx);
            modbus_tcp_accept(ctx, &server_socket); // Re-accept new connections
        }
    }

    // Cleanup (never reached in this example)
    cleanup(0);
    return 0;
}
